#include "cstanley.h"

const char fileName[] = "/home/shukebeita/data/data.txt";

CStanley::CStanley():k(0.1), Kp(0.8), dt(0.05),
    L(2.91), targetSpeed(20), idxTarget(0)
{
    posRef = readPosPoints(XRef, YRef);
    initStates = {0, 2, 0, 0}; // X, Y, Phi, v
    states = initStates;
    statesActual.push_back(initStates);
}

CStanley::~CStanley(){

}

inline double CStanley::normFunc(double posRefX, double posRefY,
                                 double posX, double posY){
    return sqrt(pow(posRefX - posX, 2) + pow(posRefY - posY, 2));
}

//读取路径点
vector<vector<double>> CStanley::readPosPoints(vector<double> &XRef, vector<double> &YRef){
    ifstream readFile;
    readFile.open(fileName);

    if(!readFile.is_open()){
        cout << "file failed !!!" << endl;
        exit(0); //表示程序正常退出
    }

    string dataBuff;
    while(getline(readFile, dataBuff)){
        string tempData;
        vector<string> tempStr;
        for(size_t i = 0; i <= dataBuff.length(); i ++){
            if(dataBuff[i] == ',' || dataBuff[i] == '\0'){
                tempStr.push_back(tempData);
                tempData.clear();
            }else{
                tempData += dataBuff[i];
            }
        }

        XRef.push_back(stod(tempStr[0]));
        YRef.push_back(stod(tempStr[1]));
    }

    readFile.close();

    vector<vector<double>> res(2, vector<double>(XRef.size(), 0));
    res[0] = XRef;
    res[1] = YRef;

    // 画出曲线，这里作为验证
    //plt::plot(XRef, YRef, "r-");
    //plt::show();

    return res;
}

//在参考路径点上，寻找距离当前点最近的距离，及所对应的索引
minLatError_idx_t CStanley::findMinLatError_idx(vector<double> states,
                                      vector<vector<double>> posRef){
    int lenOfPosRef = posRef[0].size();

    vector<double> dist;
    for(int i = 0; i < lenOfPosRef; i++){
        double normRes  = normFunc(posRef[0][i], posRef[1][i], states[0], states[1]);
        dist.push_back(normRes);
    }

    auto minLatErrorPotinter = min_element(dist.begin(), dist.end());
    double minLatError_temp = *minLatErrorPotinter; // 找到当前位置到路径点的最小距离
    int idx = distance(dist.begin(), minLatErrorPotinter); // 找到距离当前位置最近的一个参考轨迹点的序号

    minLatError_idx_t tempMinLatError_idx;
    tempMinLatError_idx.idx = idx;

    if(states[1] < posRef[1][idx])
        tempMinLatError_idx.latError = -minLatError_temp;
    else
        tempMinLatError_idx.latError = minLatError_temp;

    return tempMinLatError_idx;
}

double CStanley::stanleyControl(unsigned long idxTarget, vector<double> states,
                      double latError, vector<vector<double>> posRef, double k){
    unsigned long lenOfPosRef = posRef[0].size();
    unsigned long numOfAheadPoint = 15; // 相当于预瞄距离

    vector<double> aheadPoint(2); // 这里用该点近似算法中的P2点
    if(idxTarget < lenOfPosRef-numOfAheadPoint){
        aheadPoint = {posRef[0][idxTarget+numOfAheadPoint], posRef[1][idxTarget+numOfAheadPoint]};
    }else{
        aheadPoint = {posRef[0][lenOfPosRef-1], posRef[1][lenOfPosRef-1]};
    }

    double thetaPhi = atan2(aheadPoint[1] - states[1], aheadPoint[0] - states[0]) - states[2];
    //double thetaPhi = toPi(atan((aheadPoint[1] - states[1])/(aheadPoint[0] - states[0])) - states[2]);
    //double thetaY = atan(k*latError/states[3]);
    double thetaY = atan2(k*latError, states[3]);

    double delta = thetaPhi + thetaY;
    return delta;
}

vector<double> CStanley::updateStates(double a, vector<double> states,
                            double delta, double dt, double L){
    vector<double> tempNewStates(4);
    tempNewStates[0] = states[0] + states[3]*cos(states[2])*dt;
    tempNewStates[1] = states[1] + states[3]*sin(states[2])*dt;
    tempNewStates[2] = states[2] + states[3]*dt*tan(delta)/L;
    tempNewStates[3] = states[3] + a*dt;
    return tempNewStates;
}

void CStanley::plotRes(vector<double> XRef, vector<double> YRef,
                       vector<double> XActual, vector<double> YActual){
    vector<double> plotXActual;
    vector<double> plotYActual;
    for(unsigned long i = 0; i < XActual.size(); i++) {
        plotXActual.push_back(XActual[i]);
        plotYActual.push_back(YActual[i]);
        if (i % 10 == 0) {
            plt::clf();
            plt::named_plot("refPos", XRef, YRef, "r-");
            plt::named_plot("posActual", plotXActual, plotYActual, "b*");

            // Set x-axis to interval [0,1000000]
            plt::xlim(-10, 210);

            // Add graph title
            plt::title("Stanley");
            plt::xlabel("X");
            plt::ylabel("Y");

            // Enable legend.
            plt::legend();
            // Display plot continuously
            plt::pause(0.2);
        }
    }
    plt::show();
}
